//
// Created by 马皓然 on 2025/10/25.
//
#include "cybergear_protocol.h"
#include <string.h>
#include <math.h>

// 小米电机文档中规定的浮点数到整数的转换函数 (用于发送指令)
// 这里的转换逻辑基于文档说明，将浮点数压缩到16位或12位整数
// 注意：该算法应严格遵循 CyberGear 协议文档中给出的压缩/解压公式
static uint16_t float_to_uint(float x, float x_min, float x_max, int bits) {
    float span = x_max - x_min;
    float offset = x_min;
    return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
}

/**
 * @brief  构建 29 位 CAN 扩展帧 ID
 * @param  mode: 通信类型 (5位, Bit28~bit24)
 * @param  motor_id: 目标电机 ID (8位, Bit7~bit0)
 * @param  master_id: 主机 ID/保留位 (16位, Bit23~bit8)
 * @retval 完整的 29 位扩展 ID
 */
uint32_t CyberGear_BuildExtID(uint8_t mode, uint8_t motor_id, uint16_t master_id) {
    // 扩展 ID 结构: [ Mode(5) | Res/MasterID(16) | Data/Res(8) ]
    // 注：根据文档，29位ID结构为：
    // [28:24] Mode (5 bit)
    // [23:8] Res (16 bit) 或 Master ID (文档 Page 28 示例，但这里使用官方文档的统一结构)
    // [7:0] Motor ID (8 bit)

    uint32_t id = 0;
    id |= ((uint32_t)mode & 0x1F) << 24;      // Mode: Bit 28~24
    id |= ((uint32_t)master_id & 0xFFFF) << 8; // Res/Master ID: Bit 23~8 (这里保留为 0x00)
    id |= ((uint32_t)motor_id & 0xFF) << 0;   // Motor ID: Bit 7~0

    return id;
}

/**
 * @brief  发送运动控制指令 (通信类型 1)
 * @param  hcan: CAN 句柄
 * @param  motor_id: 目标电机 ID
 * @param  cmd: 运动控制参数结构体
 * @retval HAL_OK/HAL_ERROR
 */
uint8_t CyberGear_SendMotionCmd(CAN_HandleTypeDef *hcan, uint8_t motor_id,
                                const CyberGear_MotionCmd_t *cmd) {
    CyberGear_Message_t tx_msg;
    uint16_t p_int, v_int, kp_int, kd_int;
    uint16_t t_int; // 力矩使用 12 位，但用 uint16_t 存储

    // 1. 构建 29 位扩展 ID
    tx_msg.ID = CyberGear_BuildExtID(CYBERGEAR_MODE_MOTION_CMD, motor_id, 0);
    tx_msg.DLC = 8;

    // 2. 数据压缩 (float to uint)
    // 角度 P: 16位，范围 ±4*PI
    p_int = float_to_uint(cmd->pos_ref, P_MIN, P_MAX, 16);
    // 速度 V: 12位，范围 ±30 rad/s
    v_int = float_to_uint(cmd->vel_ref, V_MIN, V_MAX, 12);
    // 力矩 T: 12位，范围 ±10.0 Nm
    t_int = float_to_uint(cmd->tor_ff, T_MIN, T_MAX, 12);
    // Kp: 12位，范围 0~500
    kp_int = float_to_uint(cmd->kp, 0.0f, KP_MAX, 12);
    // Kd: 12位，范围 0~5
    kd_int = float_to_uint(cmd->kd, 0.0f, KD_MAX, 12);

    // 3. 8 字节数据封装 (Little-Endian)
    // Byte 0-1: 目标角度 P_int (16位)
    tx_msg.Data[0] = (uint8_t)(p_int & 0xFF);
    tx_msg.Data[1] = (uint8_t)(p_int >> 8);

    // Byte 2-3: 目标速度 V_int (低4位) + Kp_int (高8位) + Kp_int (低4位) + 力矩 T_int (低4位)
    // 由于 V, T, Kp, Kd 均为 12位，需要复杂的位操作进行拼接，以填充 8 字节
    // 官方协议格式：
    // Byte 2: V_int[7:0] (速度的低 8 位)
    tx_msg.Data[2] = (uint8_t)(v_int & 0xFF);
    // Byte 3: T_int[3:0] (力矩的低 4 位) | V_int[11:8] (速度的高 4 位)
    tx_msg.Data[3] = (uint8_t)((t_int & 0x0F) | ((v_int >> 8) << 4));

    // Byte 4: T_int[11:4] (力矩的高 8 位)
    tx_msg.Data[4] = (uint8_t)(t_int >> 4);

    // Byte 5: Kp_int[7:0] (Kp 的低 8 位)
    tx_msg.Data[5] = (uint8_t)(kp_int & 0xFF);
    // Byte 6: Kd_int[3:0] (Kd 的低 4 位) | Kp_int[11:8] (Kp 的高 4 位)
    tx_msg.Data[6] = (uint8_t)((kd_int & 0x0F) | ((kp_int >> 8) << 4));

    // Byte 7: Kd_int[11:4] (Kd 的高 8 位)
    tx_msg.Data[7] = (uint8_t)(kd_int >> 4);

    // 4. 调用底层 CAN 驱动发送
    return CyberGear_CAN_SendMessage(hcan, &tx_msg);
}


/**
 * @brief  发送使能/停止运行命令 (通信类型 3 或 4)
 * @param  hcan: CAN 句柄
 * @param  motor_id: 目标电机 ID
 * @param  cmd_mode: CYBERGEAR_MODE_ENABLE 或 CYBERGEAR_MODE_STOP
 * @retval HAL_OK/HAL_ERROR
 */
uint8_t CyberGear_SendModeControl(CAN_HandleTypeDef *hcan, uint8_t motor_id, uint8_t cmd_mode) {
    CyberGear_Message_t tx_msg;

    // 1. 构建 29 位扩展 ID (Mode=3 或 Mode=4)
    tx_msg.ID = CyberGear_BuildExtID(cmd_mode, motor_id, 0);
    tx_msg.DLC = 8;

    // 2. 数据域全为 0 (标准协议要求)
    memset(tx_msg.Data, 0, 8);

    // 3. 调用底层 CAN 驱动发送
    return CyberGear_CAN_SendMessage(hcan, &tx_msg);
}

/**
 * @brief  发送写入参数命令 (通信类型 18)
 * @param  hcan: CAN 句柄
 * @param  motor_id: 目标电机 ID
 * @param  index: 目标参数索引 (如 0x7005 for 运行模式)
 * @param  len: 数据长度 (1, 2, 或 4)
 * @param  data: 待写入数据指针
 * @retval HAL_OK/HAL_ERROR
 */
uint8_t CyberGear_SendWriteParam(CAN_HandleTypeDef *hcan, uint8_t motor_id,
                                 uint16_t index, uint8_t len, const void *data) {
    CyberGear_Message_t tx_msg;

    // 1. 构建 29 位扩展 ID (Mode=18)
    // 注：此处的 master_id 可能用于区分不同的写操作，根据文档 Page 28 示例，这里简化为 0
    tx_msg.ID = CyberGear_BuildExtID(CYBERGEAR_MODE_WRITE_PARAM, motor_id, 0);
    tx_msg.DLC = 8;

    // 2. 数据域封装
    memset(tx_msg.Data, 0, 8);

    // Byte 0-1: 写入参数的索引 (Index)
    tx_msg.Data[0] = (uint8_t)(index & 0xFF);
    tx_msg.Data[1] = (uint8_t)(index >> 8);

    // Byte 2-3: 保留

    // Byte 4-7: 待写入的数据
    if (len > 4) len = 4; // 最多 4 字节数据
    memcpy(&tx_msg.Data[4], data, len);

    // 3. 调用底层 CAN 驱动发送
    return CyberGear_CAN_SendMessage(hcan, &tx_msg);
}


